/*@brief:根据循迹传感器pid调节小车转向使小车处于黑线中间
 * @param:
 *        [in]int TraceDate: 循迹传感器输出的值
 *        [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
 * @return: 返回调节电机速度的转向pwm
 */
int ChangeTraceTurn(int TraceDate)
{
  int pwm=0;
  int bias;
  bias=TraceDate;
  pwm=PositionPID(bias,TraceTurnPdate);
  if(pwm>5000)    pwm=5000;//限幅
  else if(pwm<-5000)  pwm=-5000;
  return pwm;
}

/*@brief:根据pid调节左边电机到目标速度
 * @param:
 *        [in]int EncodeSpdL: 当前左电机编码器测速值
 *        [in]float TarSpdL:左边电机目标速度,最大速度越1.19m/s
 * @return: 返回左边电机计算后的pwm占空比
 */
int ChangeSpeedMotorL(int NowEncodeSpdL,float TarSpdL)
{
  int pwm=0;
  int bias;
  int TarEncodeSpdL;
  TarEncodeSpdL=(int)((TarSpdL*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
  bias=NowEncodeSpdL-TarEncodeSpdL;
  pwm=PositionPIDToSpd(bias,SpdPdate);
  if(pwm>5000)    pwm=5000;//限幅
  else if(pwm<-5000)  pwm=-5000;
  return pwm;
}

/*@brief:根据pid调节右边电机到目标速度
 * @param:
 *        [in]int EncodeSpdL: 当前右电机编码器测速值
 *        [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
 * @return: 返回右边电机计算后的pwm占空比
 */
int ChangeSpeedMotorR(int NowEncodeSpdR,float TarSpdR)
{
  int pwm=0;
  int bias;
  int TarEncodeSpdR;
  TarEncodeSpdR=(int)((TarSpdR*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
  bias=NowEncodeSpdR-TarEncodeSpdR;
  pwm=PositionPIDToSpd(bias,SpdPdate);
  if(pwm>5000)    pwm=5000;//限幅
  else if(pwm<-5000)  pwm=-5000;
  return pwm;
}

/*@brief: 让小车根据循迹黑线走
 *@param:
 *        [in]TraceDate: 循迹传感器输出的值
 *        [in]TarSpeed:循迹的目标速度
 *@return: 到达目标点返回1，否则返回0
 */
void TraceMove(int TraceDate,float TarSpeed)
{
  int turnpwm=0;
  int spdpwml=0,spdpwmr=0;
  int pwml=0,pwmr=0;
  
  turnpwm=ChangeTraceTurn(TraceDate);
  
  spdpwml=ChangeSpeedMotorL(Encode_Left,TarSpeed);
  spdpwmr=ChangeSpeedMotorR(Encode_Right,TarSpeed);
  
  
  pwmr=turnpwm+spdpwmr;
  if(pwmr>5000)   pwmr=5000;//限幅
  else if(pwmr<-5000)  pwmr=-5000;
  pwml=-turnpwm+spdpwml;
  if(pwml>5000)   pwml=5000;//限幅
  else if(pwml<-5000)  pwml=-5000;
  
  Set_Pwm_Motor1(pwmr);       //设置电机1占空比
  Set_Pwm_Motor2(pwml);       //设置电机2占空比
}
